import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;

import javax.swing.BorderFactory;
import javax.swing.JPanel;

import Jama.Matrix;


public class Robot{

	//Top left corner of the robot (wheel 1)
	private double position_x;
	private double position_y;
	
	private int width;
	private int height;
	
	//Amount by which car is rotated
	private double theta;
	
	Sensor s1;
	Sensor s2;
	
	private Matrix kMatrix;
	private Matrix speed; //???
	
	public Robot(int initialX, int initialY, double initTheta, int k1Val, int k2Val, int k3Val, int k4Val)
	{	
		height = 20;
		width = 40;
		
		position_x = initialX;
		position_y = initialY;
		
		double [][] k = {{k1Val, k2Val},
						{k3Val, k4Val}};
		kMatrix = new Matrix(k);
		
		s1 = new Sensor(position_x+width, position_y);
		s2 = new Sensor(position_x+width, position_y+height);
		
		theta = initTheta;
		
		speed = new Matrix(2, 1);
	}
	
	public void setPosX(int x)
	{
		position_x = x;
	}
	
	public void setPosY(int y)
	{
		position_y=y;
	}
	
	public void setTheta(double t)
	{
		theta = t;
	}
	
	// returns an angle b/t 0 and 2pi
	private double adjAngle(double a)
	{
		if (a >= 0 && a <= 2*Math.PI)
			return a;
		else if (a < 0)
			adjAngle(a+2*Math.PI);
		else if (a > 2*Math.PI)
			adjAngle(a-2*Math.PI);
		return 0; // SHOULD NOT HAPPEN!
	}
	
	// set the speed using the given light
	// BE SURE TO PASS THE CLOSEST LIGHT
	public void setSpeed(Light light)
	{
		double [][] tmp = {{s1.output(light)},
				{s2.output(light)}};
		Matrix sense = new Matrix(tmp);
		
		double tmpangle = adjAngle(theta);
		Matrix k = new Matrix(2,2);
		// HACK!
		// adjusts kMatrix based on current angle
		if ((tmpangle >= 0 && tmpangle <= Math.PI/2) || (tmpangle >= (3*(Math.PI/2)) && tmpangle <= 2*Math.PI))
		{
			k.set(0, 0, kMatrix.get(0, 1));
			k.set(0, 1, kMatrix.get(0, 0));
			k.set(1, 0, kMatrix.get(1, 1));
			k.set(1, 1, kMatrix.get(1, 0));
		}
		else if (tmpangle >= Math.PI/2 && tmpangle <= (3*(Math.PI/2)))
		{
			k = kMatrix;
		}
		
		speed = k.times(sense);
	}
	
	// Calculate and set the location 1 (??) second in the future
	// Correct speed should be set b/f calling this
	public void calcLocation()
	{
		double speedOut, speedIn;
		int time = 1; // or 1000??
		
		// both have the same speed
		if (Math.abs(speed.get(0, 0) - speed.get(1, 0)) <= 0.01)
		{
			speed.set(0, 0, speed.get(0,0)+0.01);
		}
		
		speedOut = speed.get(0, 0);
		speedIn = speed.get(1, 0);
		position_x = (position_x + ((double)height / 2) * ((speedOut + speedIn) / (speedOut - speedIn)) *
					(Math.sin((speedOut - speedIn) * (time / (double)height) + theta) - Math.sin(theta)));
		position_y = (position_y + ((double)height / 2) * ((speedOut + speedIn) / (speedOut - speedIn)) *
					(Math.cos((speedOut - speedIn) * (time / (double)height) + theta) - Math.cos(theta)));
		theta = (speedOut - speedIn) * (time / (double)height) + theta;
		
		s1.setPosX(position_x + width);
		s1.setPosY(position_y);
		s2.setPosX(position_x + width);
		s2.setPosY(position_y + height);
		//System.out.println(position_x + " " + position_y + " " + theta);
	}
	
	public Matrix getKMatrix()
	{
		return kMatrix;
	}
	
	public int getPosX()
	{
		return (int)Math.round(position_x);
	}
	
	public int getPosY()
	{
		return (int)Math.round(position_y);
	}
	
	public double getTheta()
	{
		return theta;
	}
	
	public int getWidth()
	{
		return width;
	}
	
	public int getHeight()
	{
		return height;
	}
}
